

#include "vector"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <string>
#include <chrono>
#include "yolo_detect_message/msg/call_server_status.hpp"
using namespace std::chrono_literals;



class xietongdingwei: public rclcpp::Node
{
private:
    std::vector<sensor_msgs::msg::NavSatFix> uav_gloabal_position;


// 订阅无人机的位置信息的节点

    int num_car = 3;
    std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> global_subscription_;
    std::string pub_name = "xietongdinwei";
    rclcpp::TimerBase::SharedPtr publish_timer;
    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr dingwei_pub_;
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;
    std::string status_topic = "/uav_swarms/xietongdingwei_server_topic";
    bool service_status = false;
    std::mutex service_status_mutex;

public:
    xietongdingwei(/* args */);
    ~xietongdingwei();
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    void publish_target();
};

xietongdingwei::xietongdingwei(/* args */):Node("xitongdingwei")
{

    service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(status_topic,rclcpp::QoS(20).best_effort(),std::bind(&xietongdingwei::service_status_callback,this,std::placeholders::_1));
    dingwei_pub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>(pub_name, 10);
    void publish_target();
    // 构造函数中出现
    global_subscription_.resize(num_car);

    uav_gloabal_position.resize(num_car);

    publish_timer = this->create_wall_timer(
            100ms, 
            std::bind(&xietongdingwei::publish_target, this) 
        );

    for(int i = 0; i  < 3; i++){
    std::string topic_name = "/drone_" + std::to_string(i) + "/global_position/global";
    global_subscription_[i] = this->create_subscription<sensor_msgs::msg::NavSatFix>(topic_name, rclcpp::QoS(100).best_effort(),
            [this, i](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
            
                uav_gloabal_position[i]  = *msg;
                RCLCPP_INFO(this->get_logger(),"received message is");
            });
        
    }

}

xietongdingwei::~xietongdingwei()
{
}



// 成员函数
void xietongdingwei::publish_target(){
    if (service_status == false)
    {
        return;
    }
    
    double al = 0.0;
    double lo = 0.0;
    double l = 0.0;

    for(int i = 0; i <3 ; i++){
        al = uav_gloabal_position[i].longitude + al;
        lo = uav_gloabal_position[i].latitude + lo;
        l = uav_gloabal_position[i].altitude + l;
    }
    al = al  / 3.0;
    lo = lo  / 3.0;
    l = l  / 3.0 - 80.0; 
    sensor_msgs::msg::NavSatFix result_msg;
    result_msg.longitude = al;
    result_msg.latitude = lo;
    result_msg.altitude = l;
    dingwei_pub_->publish(result_msg);
}

void xietongdingwei::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  
  bool reqs = msg->status;
    if(reqs != service_status)
    {
        std::lock_guard<std::mutex> lock_image(service_status_mutex);
        service_status = reqs;
    }
    RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service status is %d.",service_status);
}


int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<xietongdingwei>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
